from pyrobot.map.tkmap import TkMap
from math import cos, sin, pi, sqrt
import math
import copy
from pyrobot.bean.map.mapstructure import MapStructure

class LocalView(TkMap):
   """
   GUI for visualizing the local perceptual space of a robot.
   """
   def __init__(self, cols, rows, value = 0.5,
                width = 200, height = 200,
                widthMM = 9500, heightMM = 9500,
                title = "Local Perceptual Space"):
      """ Pass in grid cols, grid cells, and total width/height in MM"""
      
      # a special structure
      self.localMap = []
      # global position
      self.obstacleGlobalPos = []
      # grid list
      self.gridList = []
       
      self.step = 0
      TkMap.__init__(self, cols, rows, value,
                     width, height,
                     widthMM, heightMM, title)
      
      self.mapWidth = 1000
      self.mapHeight = 1000
      self.cellWidth = 50
      self.cellHeight = 50
      self.scaleX = 2
      self.scaleY = 2
      
      # center of view
      self.centerY = self.height/2 + 90
      self.centerX = self.width/2

   def color(self, value, maxvalue):
      value = 1.0 - value / maxvalue
      color = "gray%d" % int(value * 100.0) 
      return color

   def sensorHits(self, robot, item):
      """
      Point (0,0) is located at the center of the robot.
      Point (offx, offy) is the location of the sensor on the robot.
      Theta is angle of the sensor hit relative to heading 0.
      Dist is the distance of the hit from the sensor.
      Given these values, need to calculate the location of the hit
      relative to the center of the robot (hitx, hity).  

                    .(hitx, hity)
                   /
                  / 
                 /  
           dist /   
               /    
              /     
             /theta 
            .-------
           (offx, offy)
        
      .-->heading 0
      (0,0)
      
      """
      originalUnits = robot.__dict__[item].units
      robot.__dict__[item].units = 'METERS'
      radius = robot.radius
      # -----------------------------------
      
#      fout = open('log.txt', 'w+')
#      text = "robot.__dict__[item].count " + str(robot.__dict__[item].count) + "\n"
#      fout.write(text)
#      fout.write("robot.x " + str(robot.x) + "\n")
#      fout.write("robot.y " + str(robot.y) + "\n")
#      fout.write("robot.th " + str(robot.th) + "\n")
#      test1 = "start " + "\n"
      
      # clear local map history
      del self.localMap[:]
      del self.gridList[:]
      del self.obstacleGlobalPos[:]
      # get sense information    
      robotTheta = robot.th * math.pi / 180
      for i in range(robot.__dict__[item].count):
         # in MM:
         offx, offy, z, theta, arc = robot.__dict__[item][i].geometry
         # in METERS, because we set it so above:
         dist = robot.__dict__[item][i].value
         if dist < robot.__dict__[item].getMaxvalue():
            senseObstacle = 1
         else:
            senseObstacle = 0
         # local coordination, convert to MMs:
         
         if dist > robot.__dict__[item].getMaxvalue():
             print "set max value for dist"
             dist = robot.__dict__[item].getMaxvalue()
         
         hitx_global_angle = cos(theta+robotTheta) * dist * 1000 + offx
         hity_global_angle = sin(theta+robotTheta) * dist * 1000 + offy
         hitx_local_angle = cos(theta) * dist * 1000 + offx
         hity_local_angle = sin(theta) * dist * 1000 + offy
         # set localMap
#         self.localMap = self.localMap, (hitx_global, hity_global)
         self.localMap.append((hitx_global_angle, hity_global_angle))
         self.gridList.append((hitx_local_angle, hity_local_angle))
         self.obstacleGlobalPos.append((hitx_global_angle+robot.x*1000, hity_global_angle+robot.y*1000))
#         self.localMap.occupy(hitx, hity)
         
         # test code
#         text1 += "(" + str(hitx_local_angle) + "," + str(hity_local_angle) + ")" + ", "
         
#      fout.write(text1)
      robot.__dict__[item].units = originalUnits
#      fout.close()

   def redraw(self, drawRobot = False, drawLabels = True):
       # draw the robot
       self.canvas.create_rectangle(self.centerX, self.centerY, self.centerX+4, self.centerY+4,
                          width = 0,
                          fill='red', tag = "robot")
       
       self.canvas.delete("old")
       for i in range(self.gridList.__len__()):
           x, y = self.gridList[i]
           if y>0:
               xCord = self.centerX - y / self.cellHeight * self.scaleX
               yCord = self.centerY - x / self.cellWidth * self.scaleY
           else:               
               xCord = self.centerX + (-y) / self.cellHeight * self.scaleX
               yCord = self.centerY - x / self.cellWidth * self.scaleY
           self.canvas.create_rectangle(xCord, yCord, xCord+4, yCord+4,
                                      width = 0,
                                      fill='black', tag = "old")
       self.update_idletasks()

if __name__ == '__main__':
   lps = LocalView(10, 10)
   lps.redraw()
   lps.application = 1
   lps.mainloop()
